/*
 * @Author: bor_fish 1219854025@qq.com
 * @Date: 2025-04-24 19:56:16
 * @LastEditors: bor_fish 1219854025@qq.com
 * @LastEditTime: 2025-05-26 16:34:16
 * @FilePath: /myCAN/Diagnostic1.cpp
*/
#include "Diagnostic1.h"
#include <iostream>
#include <cstring>
#include <chrono>
#include <unordered_map>


#define DEBUG_OUT   0
// 定义状态数据结构
typedef struct {
    uint8_t emergency_stop;    // 急停信号状态
    int16_t speed;            // 反馈转速
    uint16_t bus_voltage;     // 母线电压
    uint8_t bus_current;      // 母线电流
    uint8_t motor_temp;       // 电机温度
    uint8_t driver_temp;      // 驱动器温度
} StatusData0x22;


// 定义320X23数据结构
typedef struct {
    uint8_t compensator_level;    // 补偿器液位信息
    uint8_t extension_status;     // 伸缩机构状态
    uint8_t proximity_switches;   // 接近开关状态
    uint8_t fault_stop;          // 故障位-1（停机类）
    uint8_t fault_warning;       // 故障位-2（告警通知类）
    uint8_t reserved[3];         // 备用
} StatusData0x23;

// 定义320X24数据结构
typedef struct {
    uint8_t hardware_version;     // 硬件版本号
    uint8_t software_version;     // 软件版本号
    uint8_t reserved[6];         // 备用
} StatusData0x24;

// 用static map记录每个frame_id的时间戳
static std::unordered_map<uint32_t, int64_t> last_send_time;
static std::unordered_map<uint32_t, uint32_t> feedback_to_send = {
    {0x32022, 0x32021},
    {0x32032, 0x32031},
    {0x32042, 0x32041},
    {0x32052, 0x32051},
    {0x32062, 0x32061},
    {0x32072, 0x32071},
    {0x32082, 0x32081},
    // 后续可扩展
};

/*================================================================================================*/
                                            /*诊断处理层*/
// 检查CAN帧完整性和正确性
void Diagnostic1::check_can_frame(const std::array<unsigned char, 100>& packet, uint32_t frame_id) {
    std::vector<DiagnosticError> errors;

    // 1. 检查帧ID
    if ((frame_id & 0xFFFFF000) == 0x32000) {
        uint32_t x = (frame_id >> 4) & 0xF;
        uint32_t suffix = frame_id & 0x0F;
        if (x < 2 || x > 8) {
            error_list_.push_back({frame_id, DiagnosticErrorType::InvalidFrameId, "CAN ID非法(x范围错误)"});
            return;
        }
        if (!(suffix == 0x1 || suffix == 0x2 )) {
            error_list_.push_back({frame_id, DiagnosticErrorType::InvalidFrameId, "CAN ID非法(后缀错误)"});
            return;
        }
    } else if ((frame_id & 0xFFFF0000) == 0x320000) {
        uint32_t x = (frame_id >> 8) & 0xF;
        uint32_t suffix = frame_id & 0xFF;
        if (x < 2 || x > 8) {
            error_list_.push_back({frame_id, DiagnosticErrorType::InvalidFrameId, "CAN ID非法(x范围错误)"});
            return;
        }
        if (!(suffix == 0x22 || suffix == 0x23 || suffix == 0x24)) {
            error_list_.push_back({frame_id, DiagnosticErrorType::InvalidFrameId, "CAN ID非法(后缀错误)"});
            return;
        }
    } else {
        error_list_.push_back({frame_id, DiagnosticErrorType::InvalidFrameId, "CAN ID非法(前缀错误)"});
        return;
    }

    // 2. 检查DLC
    uint8_t dlc = packet[25];
    if (dlc < 8) {
        error_list_.push_back({frame_id, DiagnosticErrorType::InvalidDLC, "DLC非法"});
        #if DEBUG_OUT
        std::cerr << "[error] DLC非法: " << std::dec << (int)dlc << std::endl;
        #endif
        // 不return，继续检查
    }

    // 4. CRC校验
    uint16_t crc_in_frame;
    std::memcpy(&crc_in_frame, &packet[12], sizeof(uint16_t));
    uint16_t crc_calc = listener_.calc_can_crc(packet.data() + 21, 13);
    if (crc_in_frame != crc_calc) {
        error_list_.push_back({frame_id, DiagnosticErrorType::CRCError, "CRC校验失败"});
        #if DEBUG_OUT
        std::cerr << "[error] CRC校验失败: 帧内=" << std::hex << crc_in_frame
        << " 计算=" << crc_calc << std::endl;
        #endif
        return;
    }
}

static void ThrusterUpperToDrive(const std::array<unsigned char, 100>& packet,uint32_t frame_id)
{

    // 提取时间戳
    int64_t timestamp_ns = 0;
    std::memcpy(&timestamp_ns, &packet[2], sizeof(int64_t));

    last_send_time[frame_id] = timestamp_ns;
    #if DEBUG_OUT
    //std::cout << "[DEBUG] 收到32021, frame_id=0x" << std::hex << frame_id << ", ts=" << timestamp_ns << std::endl;
    #endif 

}
void Diagnostic1::ThrusterDriverFeedback(const std::array<unsigned char, 100>& packet, uint32_t frame_id) {
    int64_t timestamp_ns = 0;
    std::memcpy(&timestamp_ns, &packet[2], sizeof(int64_t));

    if (feedback_to_send.count(frame_id)) {
        uint32_t send_id = feedback_to_send[frame_id];
        if (last_send_time.count(send_id)) {
            int64_t diff_ns = timestamp_ns - last_send_time[send_id];
            double diff_ms = diff_ns / 1e6;
            if (diff_ms > 2) {
                error_list_.push_back({frame_id, DiagnosticErrorType::FeedbackTimeout, "反馈超时", timestamp_ns});
                #if DEBUG_OUT
                std::cout << "[警告] 0x" << std::hex << send_id << "->0x" << frame_id << " 反馈超时: " << diff_ms << " ms" << std::endl;
                #endif
            }
            last_send_time.erase(send_id);
        }
    }
}

static void ThrusterDriverID22(const std::array<unsigned char, 100>& packet,uint32_t frame_id)
{
    #if DEBUG_OUT
    //                 std::cout << "Actor ID: 0x" << std::hex 
    //             << std::setfill('0') << std::setw(8) 
    //             << frame_id << std::endl;
    #endif
    StatusData0x22 data = {};

    // 提取急停信号状态
    data.emergency_stop = packet[26];
    // 提取反馈转速
    data.speed = (packet[27] | (packet[28] << 8));
    // 提取母线电压
    data.bus_voltage = (packet[29] | (packet[30] << 8));
    // 提取母线电流
    data.bus_current = packet[31];
    // 提取电机温度
    data.motor_temp = packet[32];
    // 提取驱动器温度
    data.driver_temp = packet[33];

    #if DEBUG_OUT
    // printf("设备 0x%X 发送状态: 急停=%d, 速度=%d, 电压=%d, 电流=%d, 电机温度=%d, 驱动器温度=%d\n",
    // frame_id, data.emergency_stop, data.speed ,
    // data.bus_voltage, data.bus_current, data.motor_temp, data.driver_temp );
    #endif

}

static void ThrusterDriverID23(const std::array<unsigned char, 100>& packet,uint32_t frame_id)
{
    StatusData0x23 data = {};
    // 提取补偿器液位信息
    data.compensator_level = packet[26];
    // 提取伸缩机构状态
    data.extension_status = packet[27];
    // 提取接近开关状态
    data.proximity_switches = packet[28];
    // 提取故障位-1（停机类）
    data.fault_stop = packet[29];
    // 提取故障位-2（告警通知类）
    data.fault_warning = packet[30];
    // 保留
    // data.reserved[0] = 0;
    // data.reserved[1] = 0;
    // data.reserved[2] = 0;

    #if DEBUG_OUT
    // printf("设备 0x%X 补偿器液位信息=%d, 伸缩机构状态=%d, 接近开关状态=%d, 故障位-1=%d, 故障位-2=%d,\n",
    // frame_id, data.compensator_level, data.extension_status ,
    // data.proximity_switches, data.fault_stop , data.fault_warning );
    #endif

}

static void ThrusterDriverID24(const std::array<unsigned char, 100>& packet,uint32_t frame_id)
{
    StatusData0x24 data = {};

    // 提取硬件版本号
    data.hardware_version = packet[26];
    // 提取软件版本号
    data.software_version = packet[27];
    // 保留
    // data.reserved[0] = 0;
    // data.reserved[1] = 0;
    // data.reserved[2] = 0;
    // data.reserved[3] = 0;
    // data.reserved[4] = 0;
    // data.reserved[5] = 0;
    
    #if DEBUG_OUT
    // printf("设备 0x%X 硬件版本号=%d, 软件版本号=%d\n",
    // frame_id, data.hardware_version, data.software_version);
    #endif
}
/*================================================================================================*/
                                            /*ID入口层*/
// ID 0x123的诊断处理函数
void Diagnostic1::processFrame32021(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterUpperToDrive(packet,frame_id);
    // DiagnosticThread::printTimestamp(packet.data());

    // // 计算时间间隔
    // int64_t time_diff = DiagnosticThread::calculateTimeInterval(packet.data()
    // , last_timestamp, first_packet);

}

// ID 0x321的诊断处理函数
void Diagnostic1::processFrame32022(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    // DiagnosticThread::printTimestamp(packet.data());
    ThrusterDriverFeedback(packet, frame_id);
    // // 计算时间间隔
    // int64_t time_diff = DiagnosticThread::calculateTimeInterval(packet.data(), 
    //                                         last_timestamp, first_packet);
    
}

void Diagnostic1::processFrame32031(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterUpperToDrive(packet,frame_id);
}

void Diagnostic1::processFrame32032(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterDriverFeedback(packet,frame_id);
}

void Diagnostic1::processFrame32041(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterUpperToDrive(packet,frame_id);
}

void Diagnostic1::processFrame32042(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterDriverFeedback(packet,frame_id);
}


void Diagnostic1::processFrame32051(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterUpperToDrive(packet,frame_id);
}

void Diagnostic1::processFrame32052(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterDriverFeedback(packet,frame_id);
}


void Diagnostic1::processFrame32061(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterUpperToDrive(packet,frame_id);
}

void Diagnostic1::processFrame32062(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterDriverFeedback(packet,frame_id);
}


void Diagnostic1::processFrame32071(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterUpperToDrive(packet,frame_id);
}

void Diagnostic1::processFrame32072(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterDriverFeedback(packet,frame_id);
}


void Diagnostic1::processFrame32081(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterUpperToDrive(packet,frame_id);
}

void Diagnostic1::processFrame32082(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterDriverFeedback(packet,frame_id);
}

void Diagnostic1::processFrame320222(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterDriverID22(packet,frame_id);
}

void Diagnostic1::processFrame320223(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterDriverID23(packet,frame_id);
}

void Diagnostic1::processFrame320224(const std::array<unsigned char, 100>& packet,uint32_t frame_id) {
    ThrusterDriverID24(packet,frame_id);
}
/*================================================================================================*/
                                            /*线程层*/
void Diagnostic1::parseData() {
    can_frame frame;
    int interface_index;
    int last_processed_index = 0;

    std::vector<DiagnosticError> all_errors;
    while(isRunning()) {
        // 记录开始时间
        auto start_time = std::chrono::steady_clock::now();
        if (listener_.getNextData(frame, interface_index, thread_Index_)) {
            int current_write_index = listener_.getWriteIndex(interface_index);
            for (int i = last_processed_index; i < current_write_index; i++) {
                int buffer_index = i % listener_.getBufferSize();
                auto& packet = listener_.getPacketBuffer()[interface_index][buffer_index];
                #if DEBUG_OUT
                // std::cout << "last_processed_index: " << last_processed_index<< std::endl;
                // std::cout << "current_write_index: " << current_write_index<< std::endl;
                #endif
                
                // 提取帧ID
                uint32_t frame_id;
                std::memcpy(&frame_id, &packet[21], sizeof(uint32_t));

                check_can_frame(packet,frame_id);
                // 只要是有诊断的地方都收集错误
                std::vector<DiagnosticError> feedback_errors;
                switch (frame_id)
                {
                case 0x32021:
                    processFrame32021(packet,frame_id);
                    break;

                case 0x32022:
                    processFrame32022(packet,frame_id);
                    break;

                case 0x32031:
                    processFrame32031(packet,frame_id);
                    break;

                case 0x32032:
                    processFrame32032(packet,frame_id);
                    break;

                case 0x32041:
                    processFrame32041(packet,frame_id);
                    break;

                case 0x32042:
                    processFrame32042(packet,frame_id);
                    break;

                case 0x32051:
                    processFrame32051(packet,frame_id);
                    break;

                case 0x32052:
                    processFrame32052(packet,frame_id);
                    break;

                case 0x32061:
                    processFrame32061(packet,frame_id);
                    break;

                case 0x32062:
                    processFrame32062(packet,frame_id);
                    break;

                case 0x32071:
                    processFrame32071(packet,frame_id);
                    break;

                case 0x32072:
                    processFrame32072(packet,frame_id);
                    break;

                case 0x32081:
                    processFrame32081(packet,frame_id);
                    break;

                case 0x32082:
                    processFrame32082(packet,frame_id);
                    break;

                case 0x320222:
                    processFrame320222(packet,frame_id);
                break;

                case 0x320223:
                    processFrame320223(packet,frame_id);
                break;

                case 0x320224:
                    processFrame320224(packet,frame_id);
                break;

                default:
                    break;
                }
                #if DEBUG_OUT
                // struct rusage usage;
                // getrusage(RUSAGE_SELF, &usage);
                // std::cout << "Memory usage: " << usage.ru_maxrss << " KB" << std::endl;
                #endif
                
            }
                // 例如在parseData最后
                for (const auto& err : error_list_) {
                    std::cerr << "[错误] ID:0x" << std::hex << err.frame_id
                            << " 类型:" << static_cast<int>(err.type)
                            << " 描述:" << err.description << std::endl;
                }
                error_list_.clear(); // 处理完清空
            last_processed_index = current_write_index;
            listener_.notifyDataProcessed();
        }
    }
}